package Engine;

import java.nio.FloatBuffer;

import org.lwjgl.BufferUtils;


public class Matrix3D {

	public float m00, m01, m02, m03;
	public float m10, m11, m12, m13;
	public float m20, m21, m22, m23;
	public float m30, m31, m32, m33;

	public Matrix3D(){
		m00 = m01 = m02 = m03 = 0.0f;
		m10 = m11 = m12 = m13 = 0.0f;
		m20 = m21 = m22 = m23 = 0.0f;
		m30 = m31 = m32 = m33 = 0.0f;
	}

	public Matrix3D(Matrix3D a){
		m00 = a.m00;
		m01 = a.m01;
		m02 = a.m02;
		m03 = a.m03;
		m10 = a.m10;
		m11 = a.m11;
		m12 = a.m12;
		m13 = a.m13;
		m20 = a.m20;
		m21 = a.m21;
		m22 = a.m22;
		m23 = a.m23;
		m30 = a.m30;
		m31 = a.m31;
		m32 = a.m32;
		m33 = a.m33;
	}
	
	public Matrix3D(Matrix3D a,Matrix3D b){
		combine(a,b);
	}

	public void setIdentity(){
		m00 = 1.0f;
		m01 = 0.0f;
		m02 = 0.0f;
		m03 = 0.0f;
		m10 = 0.0f;
		m11 = 1.0f;
		m12 = 0.0f;
		m13 = 0.0f;
		m20 = 0.0f;
		m21 = 0.0f;
		m22 = 1.0f;
		m23 = 0.0f;
		m30 = 0.0f;
		m31 = 0.0f;
		m32 = 0.0f;
		m33 = 1.0f;
	}

	public Object clone() {
		return new Matrix3D(this);
	}

	public void copy(Matrix3D input){
		m00 = input.m00;
		m01 = input.m01;
		m02 = input.m02;
		m03 = input.m03;
		m10 = input.m10;
		m11 = input.m11;
		m12 = input.m12;
		m13 = input.m13;
		m20 = input.m20;
		m21 = input.m21;
		m22 = input.m22;
		m23 = input.m23;
		m30 = input.m30;
		m31 = input.m31;
		m32 = input.m32;
		m33 = input.m33;
	}

	public double[] toArray(){
		double[] matrix = new double[16];
		matrix[0] = m00;
		matrix[1] = m01;
		matrix[2] = m02;
		matrix[3] = m03;
		matrix[4] = m10;
		matrix[5] = m11;
		matrix[6] = m12;
		matrix[7] = m13;
		matrix[8] = m20;
		matrix[9] = m21;
		matrix[10] = m22;
		matrix[11] = m23;
		matrix[12] = m30;
		matrix[13] = m31;
		matrix[14] = m32;
		matrix[15] = m33;
		return matrix;
	}

	public FloatBuffer toFloatBuffer(){
		FloatBuffer fb = BufferUtils.createFloatBuffer(16);
		fb.put(0,(float)m00);
		fb.put(1,(float)m01);
		fb.put(2,(float)m02);
		fb.put(3,(float)m03);
		fb.put(4,(float)m10);
		fb.put(5,(float)m11);
		fb.put(6,(float)m12);
		fb.put(7,(float)m13);
		fb.put(8,(float)m20);
		fb.put(9,(float)m21);
		fb.put(10,(float)m22);
		fb.put(11,(float)m23);
		fb.put(12,(float)m30);
		fb.put(13,(float)m31);
		fb.put(14,(float)m32);
		fb.put(15,(float)m33);
		return fb;
	}

	public void combine(Matrix3D m1) {
		Matrix3D tmp = new Matrix3D(this);
		combine(tmp,m1);
	}

	public void combineInverse(Matrix3D m1) {
		Matrix3D tmp = new Matrix3D(this);
		combine(m1,tmp);
	}

	public void combine(Matrix3D m1,Matrix3D m2) {
		m00 = m1.m00*m2.m00 + m1.m01*m2.m10 + m1.m02*m2.m20 + m1.m03*m2.m30;
		m01 = m1.m00*m2.m01 + m1.m01*m2.m11 + m1.m02*m2.m21 + m1.m03*m2.m31;
		m02 = m1.m00*m2.m02 + m1.m01*m2.m12 + m1.m02*m2.m22 + m1.m03*m2.m32;
		m03 = m1.m00*m2.m03 + m1.m01*m2.m13 + m1.m02*m2.m23 + m1.m03*m2.m33; m10 = m1.m10*m2.m00 + m1.m11*m2.m10 + m1.m12*m2.m20 + m1.m13*m2.m30;
		m11 = m1.m10*m2.m01 + m1.m11*m2.m11 + m1.m12*m2.m21 + m1.m13*m2.m31;
		m12 = m1.m10*m2.m02 + m1.m11*m2.m12 + m1.m12*m2.m22 + m1.m13*m2.m32;
		m13 = m1.m10*m2.m03 + m1.m11*m2.m13 + m1.m12*m2.m23 + m1.m13*m2.m33; m20 = m1.m20*m2.m00 + m1.m21*m2.m10 + m1.m22*m2.m20 + m1.m23*m2.m30;
		m21 = m1.m20*m2.m01 + m1.m21*m2.m11 + m1.m22*m2.m21 + m1.m23*m2.m31;
		m22 = m1.m20*m2.m02 + m1.m21*m2.m12 + m1.m22*m2.m22 + m1.m23*m2.m32;
		m23 = m1.m20*m2.m03 + m1.m21*m2.m13 + m1.m22*m2.m23 + m1.m23*m2.m33; m30 = m1.m30*m2.m00 + m1.m31*m2.m10 + m1.m32*m2.m20 + m1.m33*m2.m30;
		m31 = m1.m30*m2.m01 + m1.m31*m2.m11 + m1.m32*m2.m21 + m1.m33*m2.m31;
		m32 = m1.m30*m2.m02 + m1.m31*m2.m12 + m1.m32*m2.m22 + m1.m33*m2.m32;
		m33 = m1.m30*m2.m03 + m1.m31*m2.m13 + m1.m32*m2.m23 + m1.m33*m2.m33;
	}

	public void transform(Vector3D v){
		if (v==null) {
			System.out.println("Matrix4x4.transform v==null");
			return;
		}
		Vector3D temp = new Vector3D(v);
		v.x = (float)(m00 * temp.x + m01 * temp.y + m02 * temp.z + m03);
		v.y = (float)(m10 * temp.x + m11 * temp.y + m12 * temp.z + m13);
		v.z = (float)(m20 * temp.x + m21 * temp.y + m22 * temp.z + m23);
	}

	public void translate(Vector3D v){
		if (v==null) {
			System.out.println("Matrix4x4.translate v==null");
			return;
		}
		m03 += v.x;
		m13 += v.y;
		m23 += v.z;
		m33 = 1.0f;
	}

	public void setTranslate(Vector3D v){
		if (v==null) {
			System.out.println("Matrix4x4.translate v==null");
			return;
		}
		m30 += v.x;
		m31 += v.y;
		m32 += v.z;
		m33 = 1.0f;
	}

	public void scale(Vector3D v){
		m00 *= v.x;
		m01 *= v.x;
		m02 *= v.x;
		m10 *= v.y;
		m11 *= v.y;
		m12 *= v.y;
		m20 *= v.z;
		m21 *= v.z;
		m22 *= v.z;
		m33 = 1.0f;
	}

	public void scale(double scale) {
		m00 *= scale;
		m01 *= scale;
		m02 *= scale;
		m03 *= scale;
		m10 *= scale;
		m11 *= scale;
		m12 *= scale;
		m13 *= scale;
		m20 *= scale;
		m21 *= scale;
		m22 *= scale;
		m23 *= scale;
		m30 *= scale;
		m31 *= scale;
		m32 *= scale;
		m33 *= scale;
	}

	public void setRotate(Vector3D centre, float theta,float phi,float alpha)    {
		double cosAlpha, sinAlpha, cosPhi, sinPhi,
		cosTheta, sinTheta, cosPhi2, sinPhi2,
		cosTheta2, sinTheta2, c, a1,a2,a3;
		if (centre==null) {
			a1=a2=a3=0;
		} else {
			a1 = centre.x;
			a2 = centre.y;
			a3 = centre.z;
		}
		cosPhi = Math.cos(phi); sinPhi = Math.sin(phi);
		cosPhi2 = cosPhi * cosPhi; sinPhi2 = sinPhi * sinPhi;
		cosTheta = Math.cos(theta);
		sinTheta = Math.sin(theta);
		cosTheta2 = cosTheta * cosTheta;
		sinTheta2 = sinTheta * sinTheta;
		cosAlpha = Math.cos(alpha);
		sinAlpha = Math.sin(alpha);
		c = 1.0 - cosAlpha;
		m00 = (float) (cosTheta2 * (cosAlpha * cosPhi2 + sinPhi2)
				+ cosAlpha * sinTheta2);
		m10 = (float) (sinAlpha * cosPhi + c * sinPhi2 * cosTheta * sinTheta);
		m20 = (float) (sinPhi * (cosPhi * cosTheta * c - sinAlpha * sinTheta));
		m30 = 0.0f; m01 = (float) (sinPhi2 * cosTheta * sinTheta * c - sinAlpha * cosPhi);
		m11 = (float) (sinTheta2 * (cosAlpha * cosPhi2 + sinPhi2)
				+ cosAlpha * cosTheta2);
		m21 = (float) (sinPhi * (cosPhi * sinTheta * c + sinAlpha * cosTheta));
		m31 = 0.0f; m02 = (float) (sinPhi * (cosPhi * cosTheta * c + sinAlpha * sinTheta));
		m12 = (float) (sinPhi * (cosPhi * sinTheta * c - sinAlpha * cosTheta));
		m22 = (float) (cosAlpha * sinPhi2 + cosPhi2);
		m32 = 0.0f; m03 = (float) (a1 - a1 * m00 - a2 * m01 - a3 * m02);
		m13 = (float) (a2 - a1 * m10 - a2 * m11 - a3 * m12);
		m23 = (float) (a3 - a1 * m20 - a2 * m21 - a3 * m22);
		m33 = 1.0f;
	}

	public void setRotateX(double ang)    {
		double cosAng, sinAng;

		cosAng = Math.cos(ang*0.0174532);
		sinAng = Math.sin(ang*0.0174532);

		m00 = 1;
		m01 = 0;
		m02 = 0;
		m03 = 0;
		m10 = 0;
		m11 = (float) cosAng;
		m12 = (float) sinAng;
		m13 = 0;
		m20 = 0;
		m21 = (float) -sinAng;
		m22 = (float) cosAng;
		m23 = 0;
		m30 = 0;
		m31 = 0;
		m32 = 0;
		m33 = 1;
	}

	public void setRotateY(double ang)    {
		double cosAng, sinAng;

		cosAng = Math.cos(ang*0.0174532);
		sinAng = Math.sin(ang*0.0174532);

		m00 = (float) cosAng;
		m01 = 0;
		m02 = (float) -sinAng;
		m03 = 0;
		m10 = 0;
		m11 = 1;
		m12 = 0;
		m13 = 0;
		m20 = (float) sinAng;
		m21 = 0;
		m22 = (float) cosAng;
		m23 = 0;
		m30 = 0;
		m31 = 0;
		m32 = 0;
		m33 = 1;
	}  

	public void setRotateZ(double ang)    {
		double cosAng, sinAng;

		cosAng = Math.cos(ang*0.0174532);
		sinAng = Math.sin(ang*0.0174532);

		m00 = (float) cosAng;
		m01 = (float) sinAng;
		m02 = 0;
		m03 = 0;
		m10 = (float) -sinAng;
		m11 = (float) cosAng;
		m12 = 0;
		m13 = 0;
		m20 = 0;
		m21 = 0;
		m22 = 1;
		m23 = 0;
		m30 = 0;
		m31 = 0;
		m32 = 0;
		m33 = 1;
	} 

	public void setRotate(double ang,double x,double y,double z){

		double c = Math.cos(ang*0.0174532);
		double s = Math.sin(ang*0.0174532);
		double t = 1.00-c; 

		double fMag = ( x*x + y*y + z*z );
		if (fMag == 0) {return;}

		double fMult = 1.0f/Math.sqrt(fMag);            
		x *= fMult;
		y *= fMult;
		z *= fMult;

		//m( coluna)( linha)
		m00 = (float) ((t*x*x)+c);
		m01 = (float) ((t*x*y)+(s*z));
		m02 = (float) ((t*x*z)-(s*y));
		m03 = 0;
		m10 = (float) ((t*y*x)-(s*z));
		m11 = (float) ((t*y*y)+c);
		m12 = (float) ((t*y*z)+(s*x));
		m13 = 0;
		m20 = (float) ((t*z*x)+(s*y));
		m21 = (float) ((t*z*y)-(s*x));
		m22 = (float) ((t*z*z)+c);
		m23 = 0;
		m30 = 0;
		m31 = 0;
		m32 = 0;
		m33 = 1;

	} 

	public String toString(){
		String s1="["+m00+","+m01+","+m02+","+m03+"]";
		String s2="["+m10+","+m11+","+m12+","+m13+"]";
		String s3="["+m20+","+m21+","+m22+","+m23+"]";
		String s4="["+m30+","+m31+","+m32+","+m33+"]";
		return ""+s1+"\n"+s2+"\n"+s3+"\n"+s4;
	}
	public void invert() {
		float det = determinant();
		float t00 = m12*m23*m31 - m13*m22*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33    + m11*m22*m33;
		float t01 = m03*m22*m31 - m02*m23*m31 - m03*m21*m32 + m01*m23*m32 + m02*m21*m33    - m01*m22*m33;
		float t02 = m02*m13*m31 - m03*m12*m31 + m03*m11*m32 - m01*m13*m32 - m02*m11*m33    + m01*m12*m33;
		float t03 = m03*m12*m21 - m02*m13*m21 - m03*m11*m22 + m01*m13*m22 + m02*m11*m23    - m01*m12*m23;
		float t10 = m13*m22*m30 - m12*m23*m30 - m13*m20*m32 + m10*m23*m32 + m12*m20*m33    - m10*m22*m33;
		float t11 = m02*m23*m30 - m03*m22*m30 + m03*m20*m32 - m00*m23*m32 - m02*m20*m33    + m00*m22*m33;
		float t12 = m03*m12*m30 - m02*m13*m30 - m03*m10*m32 + m00*m13*m32 + m02*m10*m33    - m00*m12*m33;
		float t13 = m02*m13*m20 - m03*m12*m20 + m03*m10*m22 - m00*m13*m22 - m02*m10*m23    + m00*m12*m23;
		float t20 = m11*m23*m30 - m13*m21*m30 + m13*m20*m31 - m10*m23*m31 - m11*m20*m33    + m10*m21*m33;
		float t21 = m03*m21*m30 - m01*m23*m30 - m03*m20*m31 + m00*m23*m31 + m01*m20*m33    - m00*m21*m33;
		float t22 = m01*m13*m30 - m03*m11*m30 + m03*m10*m31 - m00*m13*m31 - m01*m10*m33    + m00*m11*m33;
		float t23 = m03*m11*m20 - m01*m13*m20 - m03*m10*m21 + m00*m13*m21 + m01*m10*m23    - m00*m11*m23;
		float t30 = m12*m21*m30 - m11*m22*m30 - m12*m20*m31 + m10*m22*m31 + m11*m20*m32    - m10*m21*m32;
		float t31 = m01*m22*m30 - m02*m21*m30 + m02*m20*m31 - m00*m22*m31 - m01*m20*m32    + m00*m21*m32;
		float t32 = m02*m11*m30 - m01*m12*m30 - m02*m10*m31 + m00*m12*m31 + m01*m10*m32    - m00*m11*m32;
		float t33 = m01*m12*m20 - m02*m11*m20 + m02*m10*m21 - m00*m12*m21 - m01*m10*m22    + m00*m11*m22;
		m00 = t00;
		m01 = t01;
		m02 = t02;
		m03 = t03;
		m10 = t10;
		m11 = t11;
		m12 = t12;
		m13 = t13;
		m20 = t20;
		m21 = t21;
		m22 = t22;
		m23 = t23;
		m30 = t30;
		m31 = t31;
		m32 = t32;
		m33 = t33;
		scale(1/det);
	}
	public void invertAffine() {
		float d = determinantAffine();
		if (d != 0.0) {
			float t00 = (m11*m22 - m12*m21) / d;
			float t01 = (m02*m21 - m01*m22) / d;
			float t02 = (m01*m12 - m02*m11) / d;
			float t10 = (m12*m20 - m10*m22) / d;
			float t11 = (m00*m22 - m02*m20) / d;
			float t12 = (m02*m10 - m00*m12) / d;
			float t20 = (m10*m21 - m11*m20) / d;
			float t21 = (m01*m20 - m00*m21) / d;
			float t22 = (m00*m11 - m01*m10) / d;
			m00 = t00;
			m01 = t01;
			m02 = t02;
			m10 = t10;
			m11 = t11;
			m12 = t12;
			m20 = t20;
			m21 = t21;
			m22 = t22;
		}
		m03 *= -1.0;
		m13 *= -1.0;
		m23 *= -1.0;
	}

	public float determinant() {
		float value;
		value =
				m03 * m12 * m21 * m30-m02 * m13 * m21 * m30-m03 * m11 * m22 * m30+m01 * m13    * m22 * m30+
				m02 * m11 * m23 * m30-m01 * m12 * m23 * m30-m03 * m12 * m20 * m31+m02 * m13    * m20 * m31+
				m03 * m10 * m22 * m31-m00 * m13 * m22 * m31-m02 * m10 * m23 * m31+m00 * m12    * m23 * m31+
				m03 * m11 * m20 * m32-m01 * m13 * m20 * m32-m03 * m10 * m21 * m32+m00 * m13    * m21 * m32+
				m01 * m10 * m23 * m32-m00 * m11 * m23 * m32-m02 * m11 * m20 * m33+m01 * m12    * m20 * m33+
				m02 * m10 * m21 * m33-m00 * m12 * m21 * m33-m01 * m10 * m22 * m33+m00 * m11    * m22 * m33;
		return value;
	}

	public void transpose() {
		float tmp = m01;
		m01 = m10;
		m10 = tmp; tmp = m02;
		m02 = m20;
		m20 = tmp; tmp = m03;
		m03 = m30;
		m30 = tmp; tmp = m12;
		m12 = m21;
		m21 = tmp; tmp = m13;
		m13 = m31;
		m31 = tmp; tmp = m23;
		m23 = m32;
		m32 = tmp;
	}

	public float determinantAffine() {
		float value;
		value = m00 * ( m11 * m22 - m21 * m12 );
		value -= m01 * ( m10 * m22 - m20 * m12 );
		value += m02 * ( m10 * m21 - m20 * m11 );
		return value;
	}
}
